#include <vector>
#include <algorithm>
#include <queue>
#include <cmath>
#include <memory>
#include <limits>
const std::vector<std::pair<int,int>> directions = {{1,0}, {0,1}, {-1,0}, {0,-1}};

template<typename T>
class PtrCompare{
public:
    bool operator()(T const * const a, T const * const b){
        return *a < *b;
    }
    bool operator()(const std::shared_ptr<T>& a, const std::shared_ptr<T>& b){
        return *a < *b;
    }
};
template<typename T>
class PtrEqual{
public:
    bool operator()(T const * const a, T const * const b){
        return *a == *b;
    }
    bool operator()(const std::shared_ptr<T>& a, const std::shared_ptr<T>& b){
        return *a == *b;
    }
};
class Node : std::enable_shared_from_this<Node> {
public:
    using Ptr=std::shared_ptr<Node>;
    int x = -1;
    int y = -1;
    int cost = std::numeric_limits<int>::max();
    int heuristic = std::numeric_limits<int>::max();
    int priority = std::numeric_limits<int>::max(); // 值越小，优先级越大
    Ptr parent;
    // 如果只允许朝上下左右四个方向运动，使用曼哈顿距离
    static int Manhattan(const Node &a, const Node &b) {
        return abs(a.x - b.x) + abs(a.y - b.y);
    }
    // 如果允许沿米字八个方向运动，使用对角距离
    static float DiagnalD(const Node &a, const Node &b) {
        int dx = abs(a.x - b.x);
        int dy = abs(a.y - b.y);
        return (sqrt(2) - 2) * std::min(dx, dy) + dx +dy;
    }
    // 如果允许朝任何方向移动，使用欧几里得距离
    static float Euclidean(const Node &a, const Node &b) {
        return sqrt(pow(a.x - b.x, 2) + pow(a.y - b.y, 2));
    }
    bool operator<(const Node &other) const {
        return priority > other.priority;
    }
    bool operator==(const Node &other) const {
        return x == other.x && y == other.y;
    }
    Ptr GetPtr(){
        return shared_from_this();
    }
    static Ptr Create(int x, int y, const Ptr& parent = nullptr){
        return Ptr(new Node(x, y, parent));
    }
    void CalcPriority(const Node &cur, const Node &end){
        cost = cur.cost + 1;
        heuristic = Manhattan(*this, end);
        priority = cost + heuristic;
    }
    void CalcPriority(const Node &cur, const std::vector<Node::Ptr> &end){
        cost = cur.cost + 1;
        std::vector<int> values;
        values.reserve(end.size());
        std::transform(end.cbegin(), end.cend(), std::back_inserter(values),
        [this](const Node::Ptr& n){return Manhattan(*this, *n);});
        heuristic = *(std::min_element(values.begin(), values.end()));
        priority = cost + heuristic;
    }
private:
    Node(): parent(nullptr) {} // 默认构造函数
    Node(int x, int y, const Ptr parent = nullptr) : x(x), y(y), parent(parent) {}
};

std::vector<std::vector<int>> BuildPath(Node::Ptr node) {
    std::vector<std::vector<int>> path;

    while (bool(node)) {
        path.emplace_back(std::vector<int>{node->x, node->y});
        node = node->parent;
    }
    std::reverse(path.begin(), path.end());
    
    return path;
}

std::vector<std::vector<int>> Astar(const std::vector<std::vector<int>> &map, const Node::Ptr& start, const std::vector<Node::Ptr>& goals) {
    std::priority_queue<Node::Ptr, std::vector<Node::Ptr>, PtrCompare<Node>> openList;
    int rows = map.size();
    if (rows == 0)
        return {};
    int cols = map[0].size();
    std::vector<std::vector<bool>> closeList(rows, std::vector<bool>(cols, false));
    std::vector<Node::Ptr> ends;
    ends.reserve(goals.size());
    for(auto& node : goals){
        if (node->x >= 0 && node->x < rows && node->y >= 0 && node->y < cols && !map[node->x][node->y])
            ends.emplace_back(node);
    }
    if(ends.size() == 0)
        return {};
    if (start->x >= 0 && start->x < rows && start->y >= 0 && start->y < cols) {
        openList.push(start);
        closeList[start->x][start->y] = true;
    }

    while (!openList.empty()) {
        Node::Ptr currentNode = openList.top();
        openList.pop();
        for(auto& node : ends){
            if (PtrEqual<Node>()(currentNode, node)) {
                return BuildPath(currentNode);
            }
        }

        for (int i = 0; i < 4; ++i) {
            int newX = currentNode->x + directions[i].first;
            int newY = currentNode->y + directions[i].second;

            if (newX >= 0 && newX < rows && newY >= 0 && newY < cols \
                && !closeList[newX][newY] && !map[newX][newY]) {
                Node::Ptr newNode = Node::Create(newX, newY, currentNode);
                newNode->CalcPriority(*currentNode, ends);
                openList.push(newNode);
                closeList[newX][newY] = true;
            }
        }
    }

    return {};
}
